/*
 * Center then drive to the end of the hall, 
 * turn around and return to the origional position.
 *
 * Uses a boolean to tell if we're turning.
 * If(turning) then do a zero-point turn.
 * This test erroneously uses the right and left sensors.
 */

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <unistd.h>

#include "args.h"

#define RAYS 32

int main(int argc, char **argv)
{
  parse_args(argc,argv);

  // we throw exceptions on creation if we fail
  try
  {
    using namespace PlayerCc;
    
    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);
    //SonarProxy sp(&robot, gIndex);
    std::cout << robot << std::endl;

    pp.SetMotorEnable (true);

    
    double newspeed = 0.5;
    double newturnrate = 0;
    //double minR = 5.6;
    //double minL = 5.6;

    robot.Read(); //here so GetRange doesn't segfault
      
    // go into read-think-act loop
    for(;;)
    {

      // this blocks until new data comes; 10Hz by default
      robot.Read();

      /* Using only the certain sensors */
      double front = lp[327];
      double right = lp[164]; //lp[218];
      double left = lp[491]; //lp[437];
      bool turning = false;

      if(turning){
	std::cout << "Turning\n";
	newspeed = 0.0;
	newturnrate = 1.5;
	//pp.SetSpeed(newspeed, newturnrate);
      }

      if(right >= 4.0){
	std::cout << "Turning\n";
	turning = true;
	newspeed = 0.0;
	newturnrate = 1.5; //180 degree turn
      }
      else if(left >= 4.0){
	std::cout << "Turning\n";
	turning = true;
	newspeed = 0.0;
	newturnrate = 1.5;
      }
      else if(right <= 4.0){
	turning = false;
	newspeed = 0.2;
	newturnrate = 0.0;
      }
      else if(left <= 4.0){
	turning = false;
	newspeed = 0.2;
	newturnrate = 0.0;
      }
      else{
	std::cout << "Uh okay\n";
      }

      printf("Front: %f\nRight: %f\nLeft: %f\n\n", front, right, left);
      pp.SetSpeed(newspeed, newturnrate);
      sleep(2);
      //getchar();
    }
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
